#include <stdio.h>
#include <string.h>
#include "commands.h"
#include "serial.h"
#include "servo.h"
#include "motor.h"
#include "stm32f10x_conf.h"

/* Convert hex string to signed short */
static s16 hex_string_to_s16(const char *hexstr)
{
  s16 value = 0;
  for(int i = 0; i < 4; i++)
  {
    value <<= 4;
    if(hexstr[i] < 0x3A)
          value += hexstr[i] - 0x30;
    else
          value += hexstr[i] - 0x37;
  }
  return value;
}

/* Process command from host */
void CMD_Process(char *cmd)
{
  int cmd_len = strlen(cmd);
  char command = cmd[0];
  unsigned char channel = cmd[1] - 0x30;
  
  switch(command)
  {
  case CMD_GET_ID:
    if(cmd_len == 1)
      putstring(ANSWER BOARD_ID, serial1);
    break;
  case CMD_SET_SPEED:
    if(cmd_len == 6)      
      SetSpeed(channel, hex_string_to_s16(cmd + 2));
    break;
  case CMD_GET_ADC:
    if(cmd_len == 2)
	printf(ANSWER "%04x\n", readADC1(channel));
    break;
  case CMD_STOP:
    if(cmd_len == 2)
      StopMotor(channel);
    else
    {
	StopMotor(1);
	StopMotor(2);
    }
    break; 
    case CMD_SET_SERVO_MAX:
    if(cmd_len == 6)
      SetServoMax(channel, hex_string_to_s16(cmd + 2));
    break;
  case CMD_GET_SERVO:
    if(cmd_len == 2)
      printf(ANSWER "%04x\n", GetServo(channel));
    break;
  case CMD_SET_SERVO:
    if(cmd_len == 10)
      SetServo(channel, hex_string_to_s16(cmd + 2), hex_string_to_s16(cmd + 6));
    break;
  case CMD_ROUTE:
    if(cmd_len >= 1 && cmd_len < MAX_ROUTED)
      putstring(cmd + 1, serial2);
    break;
  }
}
